
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       relay.c
  * @author     baiyang
  * @date       2023-9-18
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "relay.h"

#if MB_RELAY_ENABLED
#include <rtdevice.h>

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#ifndef ARRAY_SIZE
#define ARRAY_SIZE(_arr) (sizeof(_arr)/sizeof(_arr[0]))
#endif

#ifndef RELAY1_PIN_DEFAULT
  #define RELAY1_PIN_DEFAULT -1
#endif

#ifndef RELAY2_PIN_DEFAULT
  #define RELAY2_PIN_DEFAULT -1
#endif

#ifndef RELAY3_PIN_DEFAULT
  #define RELAY3_PIN_DEFAULT -1
#endif

#ifndef RELAY4_PIN_DEFAULT
  #define RELAY4_PIN_DEFAULT -1
#endif

#ifndef RELAY5_PIN_DEFAULT
  #define RELAY5_PIN_DEFAULT -1
#endif

#ifndef RELAY6_PIN_DEFAULT
  #define RELAY6_PIN_DEFAULT -1
#endif

#ifndef RELAY_AUXOUT1_PIN
  #define RELAY_AUXOUT1_PIN (0xFFFF)
#endif

#ifndef RELAY_AUXOUT2_PIN
  #define RELAY_AUXOUT2_PIN (0xFFFF)
#endif

#ifndef RELAY_AUXOUT3_PIN
  #define RELAY_AUXOUT3_PIN (0xFFFF)
#endif

#ifndef RELAY_AUXOUT4_PIN
  #define RELAY_AUXOUT4_PIN (0xFFFF)
#endif

#ifndef RELAY_AUXOUT5_PIN
  #define RELAY_AUXOUT5_PIN (0xFFFF)
#endif

#ifndef RELAY_AUXOUT6_PIN
  #define RELAY_AUXOUT6_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT1_PIN
  #define RELAY_MAINOUT1_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT2_PIN
  #define RELAY_MAINOUT2_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT3_PIN
  #define RELAY_MAINOUT3_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT4_PIN
  #define RELAY_MAINOUT4_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT5_PIN
  #define RELAY_MAINOUT5_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT6_PIN
  #define RELAY_MAINOUT6_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT7_PIN
  #define RELAY_MAINOUT7_PIN (0xFFFF)
#endif

#ifndef RELAY_MAINOUT8_PIN
  #define RELAY_MAINOUT8_PIN (0xFFFF)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    // @Param: PIN
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN,  RELAY1_PIN_DEFAULT),

    // @Param: PIN2
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN2, RELAY1_PIN_DEFAULT),

    // @Param: PIN3
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN3, RELAY1_PIN_DEFAULT),

    // @Param: PIN4
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN4, RELAY1_PIN_DEFAULT),

    // @Param: DEFAULT
    // @DisplayName: Default relay state
    // @Description: The state of the relay on boot.
    // @User: Standard
    // @Values: 0:Off,1:On,2:NoChange
    PARAM_DEFINE_INT8(RELAY_DEFAULT, 0),

    // @Param: PIN5
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN5, RELAY1_PIN_DEFAULT),

    // @Param: PIN6
    // @DisplayName: Sixth Relay Pin
    // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
    // @User: Standard
    // @Values: -1:Disabled,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
    PARAM_DEFINE_INT8(RELAY_PIN6, RELAY1_PIN_DEFAULT),
};
PARAM_GROUP_DEFINE(RELAY, var_info);

struct mb_relay relay;
/*-------------------------------------os-------------------------------------*/
static void rly_set(const uint8_t instance, const bool value);
static rt_base_t rly_get_pin(int8_t _pin);
static void rly_assign_param();
/*----------------------------------function----------------------------------*/
void rly_ctor()
{
    rly_assign_param();
}

void rly_init()
{
    if (relay._default != 0 && relay._default != 1) {
        return;
    }
    for (uint8_t i=0; i<MB_RELAY_NUM_RELAYS; i++) {
        rly_set(i, relay._default);
    }
}

// activate the relay
void rly_on(uint8_t instance) { rly_set(instance, true); }

// de-activate the relay
void rly_off(uint8_t instance) { rly_set(instance, false); }

// get state of relay
uint8_t rly_get(uint8_t instance)
{
    return instance < MB_RELAY_NUM_RELAYS ? relay._pin_states & (1U<<instance) : 0;
}

// see if the relay is enabled
bool rly_enabled(uint8_t instance)
{
    return instance < MB_RELAY_NUM_RELAYS && relay._pin[instance] != -1;
}

void rly_toggle(uint8_t instance)
{
    if (instance < MB_RELAY_NUM_RELAYS && relay._pin[instance] != -1) {
#if defined(RT_USING_PIN)
        rt_base_t pin = rly_get_pin(relay._pin[instance]);
        bool ison = rt_pin_read(pin);
        rly_set(instance, !ison);
#endif
    }
}

// check settings are valid
bool rly_arming_checks(size_t buflen, char *buffer)
{
    for (uint8_t i=0; i<MB_RELAY_NUM_RELAYS; i++) {
        int8_t pin = relay._pin[i];
        if (pin != -1 && (rly_get_pin(pin) == 0xFFFF)) {
            char param_name_buf[11] = "RELAY_PIN";
            if (i > 0) {
                rt_snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY_PIN%u", (unsigned)(i+1));
            }
// TODO: ?
#if 0
            uint8_t servo_ch;
            if (hal.gpio->pin_to_servo_channel(pin, servo_ch)) {
                rt_snprintf(buffer, buflen, "%s=%d, set SERVO%u_FUNCTION=-1", param_name_buf, (int)(pin), (unsigned)(servo_ch+1));
            } else {
                rt_snprintf(buffer, buflen, "%s=%d invalid", param_name_buf, (int)(pin));
            }
#endif
            rt_snprintf(buffer, buflen, "%s=%d invalid", param_name_buf, (int)(pin));

            return false;
        }
    }
    return true;
}

#if MB_MAVLINK_MSG_RELAY_STATUS_ENABLED
// this method may only return false if there is no space in the
// supplied link for the message.
bool rly_send_relay_status(gcs_mavlink_t gcsmav)
{
// TODO: ?
#if 0
    if (!MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, RELAY_STATUS)) {
        return false;
    }

    uint16_t present_mask = 0;
    uint16_t on_mask = 0;
    for (uint8_t i=0; i<MB_RELAY_NUM_RELAYS; i++) {
        if (!rly_enabled(i)) {
            continue;
        }
        const uint16_t relay_bit_mask = 1U << i;
        present_mask |= relay_bit_mask;

        if (relay._pin_states & relay_bit_mask) {
            on_mask |= relay_bit_mask;
        }
    }


    mavlink_msg_relay_status_send(
        gcsmav->chan,
        time_millis(),
        on_mask,
        present_mask
        );

    return true;
#else
    return false;
#endif
}
#endif  // MB_MAVLINK_MSG_RELAY_STATUS_ENABLED

static void rly_set(const uint8_t instance, const bool value)
{
    if (instance >= MB_RELAY_NUM_RELAYS) {
        return;
    }
    if (relay._pin[instance] == -1) {
        return;
    }
    const uint32_t now = time_millis();
    relay._pin_states = value ? relay._pin_states | (1U<<instance) : relay._pin_states & ~(1U<<instance);
    if ((relay._pin_states != relay._last_logged_pin_states) && (now - relay._last_log_ms > 10)) {
       //AP::logger().Write("RELY", "TimeUS,State", "s-", "F-", "QB",
       //                                 time_micros64(),
       //                                 relay._pin_states);
       relay._last_log_ms = now;
       relay._last_logged_pin_states = relay._pin_states;
    }

#if defined(RT_USING_PIN)
    rt_base_t pin = rly_get_pin(relay._pin[instance]);

    rt_pin_mode(pin, PIN_MODE_OUTPUT);
    rt_pin_write(pin, value);
#endif
}

static rt_base_t rly_get_pin(int8_t _pin)
{
    rt_base_t pin = 0xFFFF;

    switch (_pin) {
    case 101:
        pin = RELAY_MAINOUT1_PIN;
        break;

    case 102:
        pin = RELAY_MAINOUT2_PIN;
        break;

    case 103:
        pin = RELAY_MAINOUT3_PIN;
        break;

    case 104:
        pin = RELAY_MAINOUT4_PIN;
        break;

    case 105:
        pin = RELAY_MAINOUT5_PIN;
        break;

    case 106:
        pin = RELAY_MAINOUT6_PIN;
        break;

    case 107:
        pin = RELAY_MAINOUT7_PIN;
        break;

    case 108:
        pin = RELAY_MAINOUT8_PIN;
        break;

    case 50:
        pin = RELAY_AUXOUT1_PIN;
        break;

    case 51:
        pin = RELAY_AUXOUT2_PIN;
        break;

    case 52:
        pin = RELAY_AUXOUT3_PIN;
        break;

    case 53:
        pin = RELAY_AUXOUT4_PIN;
        break;

    case 54:
        pin = RELAY_AUXOUT5_PIN;
        break;

    case 55:
        pin = RELAY_AUXOUT6_PIN;
        break;

    default:
        break;
    }

    return pin;
}

static void rly_assign_param()
{
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN),  &relay._pin[0]);
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN2), &relay._pin[1]);
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN3), &relay._pin[2]);
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN4), &relay._pin[3]);
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN5), &relay._pin[4]);
    param_link_variable(PARAM_ID(RELAY, RELAY_PIN6), &relay._pin[5]);
    param_link_variable(PARAM_ID(RELAY, RELAY_DEFAULT), &relay._default);
}
/*------------------------------------test------------------------------------*/
#endif

